/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: navigationCtrl_thread.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <time.h>
#include <fcntl.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <termios.h>
#include <sys/neutrino.h>

#include "navigationCtrl_thread.h"
#include "adc.h"
#include "globaldefs.h"
#include "gps.h"
#include "utilities.h"
#include "PID.h"
#include "serial.h"

void *navigationCtrl()
{
	//Local Variables 
	struct timespec t0,t1,t0_ant,wait,next_activation,Period;
	_uint64 auxtime;
	static struct gps *gpsPacketPtr=NULL,gpsPacket;
	static struct adc *adcPacketPtr=NULL,adcPacket;
	static struct gpsAdc *gpsAdcPacketPtr=NULL,gpsAdcPacket;
	struct PIDCtrl *pidAltitudePtr=NULL;
	struct PIDCtrl *pidHeadingPtr=NULL;
	struct PIDCtrl *pidVelocityPtr=NULL;
	int fd1,fd2,sPort1,sPort2;
	long int counter;
	int counter2;
	float RollRefAux=0;
	int nbytes;
<<<<<<< .mine
	int semiAutomaticAux=0;
	float Bearing=0;
	float altRefAux=0;
	float velRefAux=0;
=======
	int semiAutomaticAux=0;
	float Bearing=0;
>>>>>>> .r976
	
	//Pointer Inicialization
	gpsPacketPtr=&gpsPacket;
	adcPacketPtr=&adcPacket;
	gpsAdcPacketPtr=&gpsAdcPacket;
	pidAltitudePtr=&pidAltitude;
	pidHeadingPtr=&pidHeading;
	pidVelocityPtr=&pidVelocity;
	
    /*********************************************************************
	 *Open and configure USB Serial Port1 (GPS)
	*********************************************************************/
	sPort1 = open_serial(SERUSB_PORT1,BAUDRATE_115200);
	gpsRequest(sPort1);
	wait.tv_sec=1;
	wait.tv_nsec=0;

	/*********************************************************************
	 *Opening Serial Port 4 (ADC)
	*********************************************************************/
	sPort2 = open_serial(SERIAL_PORT4,BAUDRATE_38400);
	adcRequest(sPort2);
	nanosleep(&wait,NULL);
	//----------------------------------------------- 
	// Main Loop
	//----------------------------------------------- 	

	counter = 0;
	counter2 = 0;
	clock_gettime(CLOCK_REALTIME, &t0_ant);
	printf ("Navigation Control Task on-line.\n");
	while(1) 
	{
		
		// Loop time claculations
		clock_gettime (CLOCK_REALTIME, &t0);
		
		// Reading GPS
		gpsAcquisition(sPort1,gpsPacketPtr);
		//printf("GPS %10f %3f \n",gpsPacket.lon,gpsPacket.vn);
		
		// Reading ADC
		adcAcquisition(sPort2,adcPacketPtr);		
		//printf("airspeed: %f\n",adcPacket.airSpeed);
		//printf("Altitude: %f\n",adcPacket.pAltitude);
		
		// Clear Buffers
		tcflush(sPort1, TCIFLUSH);
		tcflush(sPort2, TCIFLUSH);
		
		// Request Information from GPS and ADC
		gpsRequest(sPort1);
		adcRequest(sPort2);
		

		// Time Calculation -> Next Activation
		next_activation=sum_tiempos(t0,Ts_navigationCtrl); 
		
<<<<<<< .mine
		// Check Waypoint
		pthread_mutex_lock(&mutexGlobalVar);
		if(semiAutomaticAux)
		{
			//-------------------------------------------------------------------------------------------
			//									PRUEBA ROLL
			//-------------------------------------------------------------------------------------------

			/*counter2 = counter2 + 1;
			// Right Turns
			if((counter2>0)&&(counter2<21))
				RollRefAux = 0;
			if((counter2>20)&&(counter2<33))
				RollRefAux = 0.18;
			if((counter2>32)&&(counter2<53))
				RollRefAux = 0;
			if((counter2>52)&&(counter2<65))
				RollRefAux = 0.18;
			if((counter2>64)&&(counter2<85))
				RollRefAux = 0;
			if((counter2>84)&&(counter2<97))
				RollRefAux = 0.18;
			if((counter2>96)&&(counter2<117))
				RollRefAux = 0;
			if((counter2>116)&&(counter2<129))
				RollRefAux = 0.18;
			// Left Turns
			if((counter2>128)&&(counter2<149))
				RollRefAux = 0;
			if((counter2>148)&&(counter2<161))
				RollRefAux = -0.18;
			if((counter2>160)&&(counter2<181))
				RollRefAux = 0;
			if((counter2>180)&&(counter2<193))
				RollRefAux = -0.18;
			if((counter2>192)&&(counter2<213))
				RollRefAux = 0;
			if((counter2>212)&&(counter2<225))
				RollRefAux = -0.18;
			if((counter2>224)&&(counter2<245))
				RollRefAux = 0;
			if((counter2>244)&&(counter2<267))
				RollRefAux = -0.18;
			if(counter2>266)
				counter2 = 0;*/
		//--------------------------------------------------------------------------------------------------
			
			if (((fabs(flightPlan[actualWayPoint].latRef-gpsPacket.lat)< errorWayPoint)&&(fabs(flightPlan[actualWayPoint].lonRef-gpsPacket.lon)< errorWayPoint)))
			{
				
				if (actualWayPoint == (totalWayPoints-1))
					actualWayPoint = actualWayPoint;
				else
					actualWayPoint = actualWayPoint + 1;
			}	
			if ((flightPlan[actualWayPoint].latRef==NULL)&&(flightPlan[actualWayPoint].lonRef==NULL))
			actualWayPoint =  totalWayPoints-1;
			
		}

		
		pthread_mutex_lock(&mutexGlobalVar);
		Bearing = (atan2f((flightPlan[actualWayPoint].lonRef-gpsPacket.lon),(flightPlan[actualWayPoint].latRef-gpsPacket.lat)))-rotMatYawHeading.Heading;
		if (Bearing > pi)
			Bearing = Bearing - 2*pi;
		if (Bearing < -pi)
			Bearing = 2*pi + Bearing;
		altRefAux = flightPlan[actualWayPoint].altRef;
		velRefAux = flightPlan[actualWayPoint].velRef;
		pthread_mutex_unlock(&mutexGlobalVar);
		
		pthread_mutex_lock(&mutexPidHighLevel);		
		// PIDs References Update
		pidHeading.Signals.R = 0;
		pidAltitude.Signals.R = altRefAux;
		pidVelocity.Signals.R = velRefAux;
		
=======
		// Check Waypoint
		pthread_mutex_lock(&mutexGlobalVar);
		if(semiAutomaticAux)
		{
			//-------------------------------------------------------------------------------------------
			//									PRUEBA ROLL
			//-------------------------------------------------------------------------------------------

>>>>>>> .r976
			/*counter2 = counter2 + 1;
			// Right Turns
			if((counter2>0)&&(counter2<21))
				RollRefAux = 0;
			if((counter2>20)&&(counter2<33))
				RollRefAux = 0.18;
			if((counter2>32)&&(counter2<53))
				RollRefAux = 0;
			if((counter2>52)&&(counter2<65))
				RollRefAux = 0.18;
			if((counter2>64)&&(counter2<85))
				RollRefAux = 0;
			if((counter2>84)&&(counter2<97))
				RollRefAux = 0.18;
			if((counter2>96)&&(counter2<117))
				RollRefAux = 0;
			if((counter2>116)&&(counter2<129))
				RollRefAux = 0.18;
			// Left Turns
			if((counter2>128)&&(counter2<149))
				RollRefAux = 0;
			if((counter2>148)&&(counter2<161))
				RollRefAux = -0.18;
			if((counter2>160)&&(counter2<181))
				RollRefAux = 0;
			if((counter2>180)&&(counter2<193))
				RollRefAux = -0.18;
			if((counter2>192)&&(counter2<213))
				RollRefAux = 0;
			if((counter2>212)&&(counter2<225))
				RollRefAux = -0.18;
			if((counter2>224)&&(counter2<245))
				RollRefAux = 0;
			if((counter2>244)&&(counter2<267))
				RollRefAux = -0.18;
			if(counter2>266)
				counter2 = 0;*/
		//--------------------------------------------------------------------------------------------------
			
			if (((fabs(flightPlan[actualWayPoint].latRef-gpsPacket.lat)< errorWayPoint)&&(fabs(flightPlan[actualWayPoint].lonRef-gpsPacket.lon)< errorWayPoint)))
			{
				
				if (actualWayPoint == (totalWayPoints-1))
					actualWayPoint = actualWayPoint;
				else
					actualWayPoint = actualWayPoint + 1;
			}	
			if ((flightPlan[actualWayPoint].latRef==NULL)&&(flightPlan[actualWayPoint].lonRef==NULL))
			actualWayPoint =  totalWayPoints-1;
			
		}
		pthread_mutex_unlock(&mutexGlobalVar);
		pthread_mutex_lock(&mutexPidHighLevel);		
		// PIDs References Update
		pthread_mutex_lock(&mutexGlobalVar);
		pidAltitude.Signals.R = flightPlan[actualWayPoint].altRef;
		//pidAltitude.Signals.R = 350;
		pidVelocity.Signals.R = flightPlan[actualWayPoint].velRef;
		pidHeading.Signals.R = 0;
		pthread_mutex_unlock(&mutexGlobalVar);

		// PIDs Signals Update
<<<<<<< .mine
		pidHeading.Signals.y = Bearing;
=======
		//printf("NAV Update Yaw: %f \n",HeadingAux);
>>>>>>> .r976
		pidAltitude.Signals.y = adcPacket.pAltitude;
		pthread_mutex_lock(&mutexGlobalVar);
		Bearing = (atan2f((flightPlan[actualWayPoint].lonRef-gpsPacket.lon),(flightPlan[actualWayPoint].latRef-gpsPacket.lat)))-rotMatYawHeading.Heading;
		if (Bearing > pi)
			Bearing = Bearing - 2*pi;
		if (Bearing < -pi)
			Bearing = 2*pi + Bearing;
		pidHeading.Signals.y = Bearing;
		pthread_mutex_unlock(&mutexGlobalVar);
		pidVelocity.Signals.y = adcPacket.airSpeed;

		// PID Altitude Control
		PIDCalculateOutput(pidAltitudePtr,1);
		PIDUpdateStates(pidAltitudePtr,semiAutomaticAux);
		
		// PID Heading Control
		PIDCalculateOutput(pidHeadingPtr,-1);
		PIDUpdateStates(pidHeadingPtr,semiAutomaticAux);
		
		// PID Velocity Control
		PIDCalculateOutput(pidVelocityPtr,1);
		PIDUpdateStates(pidVelocityPtr,semiAutomaticAux);
		pthread_mutex_unlock(&mutexPidHighLevel);	
		
<<<<<<< .mine
		// Update Low-Level PIDs References
		pthread_mutex_lock(&mutexGlobalVar);
		rotMatYawHeading.vn = gpsPacket.vn;
		rotMatYawHeading.ve = gpsPacket.ve;
		rotMatYawHeading.vd = gpsPacket.vd;
		//printf("NAV Yaw: %f \n",rotMatYawHeading.Heading);
		pthread_mutex_unlock(&mutexGlobalVar);
			
		pthread_mutex_lock(&mutexMachineState);
		semiAutomaticAux = semiAutomatic;
		pthread_mutex_unlock(&mutexMachineState);
			
		pthread_mutex_lock(&mutexPidLowLevel);
		pthread_mutex_lock(&mutexPidHighLevel);
		//pidRoll.Signals.R = pidHeading.Signals.u;
		pidRoll.Signals.R = pidHeading.Signals.u;
		pidPitch.Signals.R = pidAltitude.Signals.u;
		pthread_mutex_unlock(&mutexPidHighLevel);
		pthread_mutex_unlock(&mutexPidLowLevel);
		
		// Save Data in FIFOs
		pthread_mutex_lock(&mutexPidHighLevel);
=======
		// Update Low-Level PIDs References
		pthread_mutex_lock(&mutexGlobalVar);
		rotMatYawHeading.vn = gpsPacket.vn;
		rotMatYawHeading.ve = gpsPacket.ve;
		rotMatYawHeading.vd = gpsPacket.vd;
		//printf("NAV Yaw: %f \n",rotMatYawHeading.Heading);

		pthread_mutex_unlock(&mutexGlobalVar);
			
		pthread_mutex_lock(&mutexMachineState);
		semiAutomaticAux = semiAutomatic;
		pthread_mutex_unlock(&mutexMachineState);
			
		pthread_mutex_lock(&mutexPidLowLevel);
		pthread_mutex_lock(&mutexPidHighLevel);
		//pidRoll.Signals.R = pidHeading.Signals.u;
		pidRoll.Signals.R = RollRefAux;
		pidPitch.Signals.R = pidAltitude.Signals.u;
		pthread_mutex_unlock(&mutexPidHighLevel);
		pthread_mutex_unlock(&mutexPidLowLevel);
		
		// Save Data in FIFOs
		pthread_mutex_lock(&mutexPidHighLevel);
>>>>>>> .r976
		counter = counter + 1;
		gpsAdcPacket.lat = gpsPacket.lat;
		gpsAdcPacket.lon = gpsPacket.lon;
		gpsAdcPacket.alt_msl = gpsPacket.alt_msl;
		gpsAdcPacket.ve = gpsPacket.ve;
		gpsAdcPacket.vn = gpsPacket.vn;
		gpsAdcPacket.vd = gpsPacket.vd;
		
<<<<<<< .mine
		gpsAdcPacket.airSpeed = adcPacket.airSpeed;
		gpsAdcPacket.RefairSpeed = pidVelocity.Signals.R;
		gpsAdcPacket.uairSpeed = pidVelocity.Signals.u;
		
		gpsAdcPacket.pAltitude = adcPacket.pAltitude;
		gpsAdcPacket.RefpAltitude = pidAltitude.Signals.R;
		gpsAdcPacket.upAltitude = pidAltitude.Signals.u;
				
		gpsAdcPacket.Ts = ((int)Period.tv_nsec)/prec;
		gpsAdcPacket.counter = counter;
		pthread_mutex_unlock(&mutexPidHighLevel);
		
		pthread_mutex_lock(&mutexGlobalVar);
		gpsAdcPacket.latRef = flightPlan[actualWayPoint].latRef;
		gpsAdcPacket.lonRef = flightPlan[actualWayPoint].lonRef;
		pthread_mutex_unlock(&mutexGlobalVar);
		
		// Datalogging FIFO
		fd1 = open(fifoNavDatalog,O_WRONLY | O_NONBLOCK);
		if (fd1>0){
			//printf("[Nav FIFO open] %d \n",fd1);
			nbytes=write(fd1,gpsAdcPacketPtr,sizeof(struct gpsAdc));
			//printf("[Nav FIFO write] %d \n",nbytes);
			close(fd1);
=======
		gpsAdcPacket.airSpeed = adcPacket.airSpeed;
		gpsAdcPacket.RefairSpeed = pidVelocity.Signals.R;
		gpsAdcPacket.uairSpeed = pidVelocity.Signals.u;
		
		gpsAdcPacket.pAltitude = adcPacket.pAltitude;
		gpsAdcPacket.RefpAltitude = pidAltitude.Signals.R;
		gpsAdcPacket.upAltitude = pidAltitude.Signals.u;
				
		gpsAdcPacket.Ts = ((int)Period.tv_nsec)/prec;
		gpsAdcPacket.counter = counter;
		pthread_mutex_unlock(&mutexPidHighLevel);
		
		// Datalogging FIFO
		fd1 = open(fifoNavDatalog,O_WRONLY | O_NONBLOCK);
		if (fd1>0){
			//printf("[Nav FIFO open] %d \n",fd1);
			nbytes=write(fd1,gpsAdcPacketPtr,sizeof(struct gpsAdc));
			//printf("[Nav FIFO write] %d \n",nbytes);
			close(fd1);
>>>>>>> .r976
		}

		// Telemetry FIFO
		fd2 = open(fifoNavTele,O_WRONLY | O_NONBLOCK);
		if (fd2>0){
			//printf("[Nav FIFO open] %d \n",fd);
			nbytes=write(fd2,gpsAdcPacketPtr,sizeof(struct gpsAdc));
			//printf("[Nav FIFO write] %d \n",nbytes);
			close(fd2);
		}
	
		// Print the results
		//if(v) printf("Inicialization--> WCCT: %3d Period: %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)t0.tv_nsec-(int)t0_ant.tv_nsec)/prec);
		//if(v) printf("N %3d %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)Period.tv_nsec)/prec);
<<<<<<< .mine

		//printf("NAV Altitude Ref: %f \n",pidAltitude.Signals.R);
		//printf("NAV Altitude Out: %f \n",pidAltitude.Signals.y);
		//printf("NAV Altitude Error: %f \n",pidAltitude.States.e);
		//printf("NAV Altitude Action: %f \n",pidAltitude.Signals.u);
		// Wait until the next activation and Calculate Period


		Period = diff_tiempos(ID_navigationCtrl,t0,t0_ant,1);
		t0_ant = t0;
=======

		
		// Wait until the next activation and Calculate Period


		Period = diff_tiempos(ID_navigationCtrl,t0,t0_ant,1);
		t0_ant = t0;
>>>>>>> .r976
		clock_gettime (CLOCK_REALTIME, &t1);
		if (timespec2nsec(&next_activation)>timespec2nsec(&t1))
		{
		    auxtime = (timespec2nsec(&next_activation)-timespec2nsec(&t1));
			nsec2timespec(&wait,auxtime);
			nanosleep(&wait, NULL);
		}	

	}
	/*********************************************************************
	 *Close USB Serial Port1 (GPS) and Serial Port 4 (ADC)
	 *********************************************************************/
	    close(sPort1);
	    close(sPort2);
	    
	return(NULL);
}